/*
* Copyright 2008  Rice University
* All Rights Reserved
* 
*       Created by Erion Plaku, Lydia Kavraki
*       Physical and Biological Computing Group
*       Department of Computer Science
*       Rice University
*/

/**
*@file PQPCollisionDetector3D.H
*@author Erion Plaku 
*@brief Wrapper for the PQP collision detection library -- 3D 
*/

#ifndef OOPSMP_UTILS_GEOMETRY_COLLISION_PQP_COLLISION_DETECTOR_PQP_COLLISION_DETECTOR_ARTICULATED_3D_H_
#define OOPSMP_UTILS_GEOMETRY_COLLISION_PQP_COLLISION_DETECTOR_PQP_COLLISION_DETECTOR_ARTICULATED_3D_H_

#include "OOPSMP/Utils/Geometry/Collision/CollisionDetector.H"
#include "OOPSMP/Utils/Geometry/Workspace/Part.H"
#include "OOPSMP/Utils/Geometry/Workspace/ArticulatedPart.h"
#include "OOPSMP/Utils/Geometry/Workspace/ArticulatedPart3D.h"
#include "PQP.h"

namespace OOPSMP
{
	///@cond
	/**
	*@brief Macro for class, class pointer, and const class pointer declarations
	*/  
	ForwardClassDeclaration(PQPCollisionDetectorArticulatedPart3D);
	///@endcond

	/**
	*@brief Wrapper for the PQP collision detection library -- 3D
	*
	*@par Plug-and-play functionality:
	*  @code
	*  BeginImplementInstanceFactory(PQPCollisionDetector3D, PQPCollisionDetectorFactory);
	*  EndImplementInstanceFactory(PQPCollisionDetector3D);
	*  @endcode      
	*/ 
	class PQPCollisionDetectorArticulatedPart3D : public CollisionDetector
	{
	public:
		/**
		*@brief Initialize data elements to appropriate default values
		*/
		PQPCollisionDetectorArticulatedPart3D(void) : OOPSMP::CollisionDetector()
		{
			m_pqpStaticPart = NULL;
		}

		/**
		*@brief Free the memory allocated to different data elements
		*/
		~PQPCollisionDetectorArticulatedPart3D(void)
		{
#ifdef HAVE_PQP 
			if(m_pqpStaticPart)
			{
				delete m_pqpStaticPart->m_model;
				free(m_pqpStaticPart);
			}
			for(int i = 0; i < (int) m_pqpMovingParts.size(); i++)
			{
				delete m_pqpMovingParts[i]->m_model;
				free(m_pqpMovingParts[i]);
			}
#else
			printf("Error: missing PQP library ... aborting\n");
			assert(0);
#endif
		}

		virtual void transformMovingPart(const int pid, const double * const trans);

	protected:

		virtual void addWorkspace(Workspace_t workspace);

		/**
		*@brief Returns true iff the indicated moving parts are in collision with each other
		*@param pid1 moving part id
		*@param pid2 moving part id
		*/
		bool isMovingPartInCollisionWithMovingPart(const int pid1, const int pid2);

		/**
		*@brief Returns true iff the indicated moving part is in collision with some static part
		*@param pid moving part id
		*/
		bool isMovingPartInCollisionWithSomeStaticPart(const int pid);

		/**
		*@brief PQP part
		*/
		struct PQPPart_struct_t
		{
			/**
			*@brief PQP model
			*/
			PQP_Model *m_model;

			/**
			*@brief PQP rotation matrix
			*/
			double     R[3][3];

			/**
			*@brief PQP translation matrix
			*/
			double     T[3];
		};

		/**
		*@brief Pointer to PQP part
		*/
		typedef PQPPart_struct_t* PQPPart_t;

		/**
		*@brief Constant pointer to PQP part
		*/
		typedef const PQPPart_struct_t* PQPPart_const_t;

		/**
		*@brief Vector of moving PQP parts
		*/
		std::vector<PQPPart_t> m_pqpMovingParts;

		/**
		*@brief PQP static part
		*/
		PQPPart_t m_pqpStaticPart;

		/**
		*@brief Add triangles of the workspace part to PQP part
		*@param pqpPart PQP part
		*@param part workspace part
		*@param id id of workspace part
		*/

		/**
		*@brief Add triangles of the workspace part to PQP part
		*@param pqpPart PQP part
		*@param part workspace part
		*@param id id of workspace part
		*/
		int addTrianglesToPQPModel(PQPPart_t pqpPart, Part_const_t part, const int id);

		/**
		*@brief Set the transformation of the PQP part as specified by <em>trans</em>
		*@param pqpPart PQP part
		*@param trans transformation
		*/
		void setPQPTransformation(PQPPart_t pqpPart, const double * const trans);

		std::vector<int> v_movingPartLength;
	};

	/**
	*@brief Declare factory and specify in source file the functions that should
	*       be registered for plug-and-play functionality
	*/  
	DeclareInstanceFactory(PQPCollisionDetectorArticulatedPart3D, CollisionDetectorFactory);
}

#endif








